#include "app.h"
#include "sht3x_dis.h"
#include "sensor_service.h"
#include "stm32l0xx_nucleo.h"
#include "math.h"
#include "lis2dh12.h"
#include "stm32l052_lp_mode.h"
#include "stm32_bluenrg_ble.h"
#include "x_nucleo_iks01a1.h"
#include "flash.h"
#include "bootloader.h"
#include "crc.h"
#include "fft.h"
/*private variable************************************************************************/
extern float sensitivity;
/*ota*/
extern uint16_t crc_serv_handle, crc_read_char_handle;
extern uint16_t file_tx_serv_handle, file_tx_write_char_handle, file_tx_notify_char_handle; 
/*sensor*/
static uint32_t viberate_value[3] = {0, 0, 0};
static axis_average_value_t tilt_average;
static int16_t fftXOutput[TEST_LENGTH_SAMPLES/2];
static uint8_t wake_up_flag = 0;
axis_type_t axis_buf[AXIS_BUFFER_SIZE] = {0};
/*private function************************************************************************/
static APP_Status_t app_tilt_angle_arithmatic(axis_average_value_t* data);
static APP_Status_t app_viberate_arithmatic(uint32_t* data);
static int32_t app_int_sqrt(uint32_t data);
static APP_Status_t app_write_new_firmware_to_flash(uint8_t* data, uint8_t len, uint32_t address);
static void app_new_firmware_manager(uint8_t*data, uint8_t len);
static void app_file_flash_write_over(void);
#ifdef APP_VIBERATE_ABS
static APP_Status_t app_viberate_abs(uint32_t* data);
#endif
/*test function*/
static void app_ble_write_flash_crc_test_1_record_data(uint32_t* data, uint32_t address);

static void app_bit_shift_to_byte(uint8_t* des_data, uint32_t* src_data, uint8_t byte_num)
{
    uint8_t i;
    
    for(i = 0; i < byte_num; i++)
    {
        des_data[i] = (*src_data >> (8 * (byte_num - i -1))) & 0xff;
    }
}

static void app_bit_shift_uint8_to_uint32(uint8_t* src_data, uint32_t* des_data)
{
     *des_data = ((src_data[0] << 24) & 0xff000000) | ((src_data[1] << 16) & 0xff0000)
                            | ((src_data[2] << 8) & 0xff00) | (src_data[3] & 0xff);
}

void app_excute_one_time_in_standby_mode(void)
{
#ifdef BlUE_NRG
    bluenrg_start_work();
#endif
}

void app_wakeup_flag_set(uint8_t data)
{
    wake_up_flag = data;
}

uint8_t app_wakeup_flag_get(void)
{
    return wake_up_flag;
}

void app_3axis_int_callback(void)
{
    app_wakeup_flag_set(APP_WAKEUP_STATES_3AXIS);
}

void app_wakeup_handle(void)
{
    uint8_t low_power_wake_up_flag = 0;
    
    low_power_wake_up_flag = app_wakeup_flag_get();
    if(low_power_wake_up_flag == APP_WAKEUP_STATES_3AXIS){
#ifdef APP_TILT_ANGLE
        app_update_adv_tilt_angle();
        HAL_Delay(20);
        bluenrg_stop_adv();
#endif
#ifdef APP_VIBERATE
        app_update_adv_viberate_value();
        HAL_Delay(20);
        bluenrg_stop_adv();
#endif
#ifdef APP_FFT
        app_update_adv_fft_value();
        HAL_Delay(20);
        bluenrg_stop_adv();
#endif     
     
#ifdef STANDBY_MODE 
       lp_set_wakeup_time(2000);
       lp_standby_mode();
#endif
    }
    
    if(low_power_wake_up_flag == APP_WAKEUP_STATES_TEMP_HUM){
       app_update_adv_tmp_hum_value();
       HAL_Delay(20);
       bluenrg_stop_adv();
       lp_set_wakeup_time(100);
#ifdef STANDBY_MODE 
       lp_set_wakeup_time(2000);
       lp_standby_mode();
#endif
    }
}

void app_update_adv_tmp_hum_value(void)
{
    uint16_t temp = 0, hum = 0;
    uint8_t buf[10] = {10};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 9;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_TEMP_HUMI_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_TEMP_HUMI_TYPE & 0xff;
    
    /*get temp, humi value*/
    sht3x_dis_read_single_slot_command(&temp, &hum);
    manuf_data->data[0] = temp >> 8;
    manuf_data->data[1] = temp & 0xff;                    
    manuf_data->data[2] = hum >> 8;
    manuf_data->data[3] = hum & 0xff;                    
                            
    if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 10) != 0x00)
         while(1);
}

void app_update_adv_viberate_value(void)
{
    uint8_t buf[18] = {0};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 17;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_VIBERATE_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_VIBERATE_TYPE & 0xff;
                             
    /*get vibereate value*/
    app_bit_shift_to_byte(&manuf_data->data[0], &viberate_value[0], 4);                          
    app_bit_shift_to_byte(&manuf_data->data[4], &viberate_value[1], 4);                          
    app_bit_shift_to_byte(&manuf_data->data[4+4], &viberate_value[2], 4);                                                    
                            
    if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 18) != 0x00)
         while(1);
}

void app_update_adv_tilt_angle(void)
{
    uint8_t buf[18] = {0};
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 17;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_TILT_ANGLE_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_TILT_ANGLE_TYPE & 0xff;
    /*get vibereate value*/
    app_bit_shift_to_byte(&manuf_data->data[0], (uint32_t*)&tilt_average.average_x, 4);                          
    app_bit_shift_to_byte(&manuf_data->data[4], (uint32_t*)&tilt_average.average_y, 4);                          
    app_bit_shift_to_byte(&manuf_data->data[4+4], (uint32_t*)&tilt_average.average_z, 4);                                                    
      
   if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 18) != 0x00)
         while(1);
}

void app_update_adv_fft_value(void)
{
    uint8_t buf[22] = {0};
    uint8_t i = 0;
    
    adv_manufactory_data_t* manuf_data = (void*)buf;
    manuf_data->len = 21;
    manuf_data->adv_type = AD_TYPE_MANUFACTURER_SPECIFIC_DATA;
    manuf_data->company_id[0] = 0xff;
    manuf_data->company_id[1] = 0xff;
    manuf_data->data_type[0] = (APP_ADV_SENSOR_FFT_TYPE >> 8) & 0xff;
    manuf_data->data_type[1] = APP_ADV_SENSOR_FFT_TYPE & 0xff;
    /*get fft value*/
    for(i = 0; i < (TEST_LENGTH_SAMPLES / 2); i++){
        manuf_data->data[i] = (fftXOutput[i] >> 8) & 0xff;
    }
    
   if(bluenrg_adv_manufacture_data((uint8_t*)manuf_data, 22) != 0x00)
         while(1);
}

void app_viberate_value_output(void)
{        
    app_viberate_arithmatic(viberate_value);
}

void app_tilt_angle_output(void)
{
    app_tilt_angle_arithmatic(&tilt_average);
}

void app_fft_output(void)
{
    uint8_t ret = 0;
    uint16_t i = 0,j = 0;

    int16_t inputData[512];
    int16_t outputData[512];
    
//    for(i = 0; i < TEST_LENGTH_SAMPLES; i++)
//        inputData[i] = axis_buf[i].axis_x;


        for(j = 0; j < 4; j++){
            for(i = 0; i < 128; i++){
//                inputData[(j * 128) + i] = sinTable_q15[i*4];
                if(i < 64)
                    inputData[(j * 128) + i] = (sinTable_q15[i*4] >> 2) + (sinTable_q15[i*8] >> 2);
                else
                    inputData[(j * 128) + i] = (sinTable_q15[i*4] >> 2) + (sinTable_q15[i*8-512] >> 2);
            }
        }
        ret = fft_q15(inputData, 512, outputData);
        if(ret != app_ok)
            while(1);
       
//    for(i = 0; i < (TEST_LENGTH_SAMPLES / 2); i++)
//        fftXOutput[i] = outputData[i] + outputData[i + 1];
    
}

static APP_Status_t app_tilt_angle_arithmatic(axis_average_value_t* data)
{
    int16_t i;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
 
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   data->average_x = sum_x / AXIS_BUFFER_SIZE;
   data->average_y = sum_y / AXIS_BUFFER_SIZE;
   data->average_z = sum_z / AXIS_BUFFER_SIZE;
    
   return app_ok;
 
}

static APP_Status_t app_viberate_arithmatic(uint32_t* data)
{
    int16_t i;
    int32_t average_x = 0, average_y = 0, average_z = 0;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
    uint32_t sum_sq_x = 0, sum_sq_y = 0, sum_sq_z = 0, average_sq_x = 0, average_sq_y = 0, average_sq_z = 0;
    
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   average_x = sum_x / AXIS_BUFFER_SIZE;
   average_y = sum_y / AXIS_BUFFER_SIZE;
   average_z = sum_z / AXIS_BUFFER_SIZE;
   /*sum of square*/
   for(i = 0; i < AXIS_BUFFER_SIZE; i++){
       sum_sq_x += ((int32_t)axis_buf[i].axis_x-average_x) * ((int32_t)axis_buf[i].axis_x-average_x);
       sum_sq_y += ((int32_t)axis_buf[i].axis_y-average_y) * ((int32_t)axis_buf[i].axis_y-average_y);
       sum_sq_z += ((int32_t)axis_buf[i].axis_z-average_z) * ((int32_t)axis_buf[i].axis_z-average_z);
   }
   /*average*/
   average_sq_x = sum_sq_x / AXIS_BUFFER_SIZE;
   average_sq_y = sum_sq_y / AXIS_BUFFER_SIZE;
   average_sq_z = sum_sq_z / AXIS_BUFFER_SIZE;
   /*sqrt*/
   data[0] = app_int_sqrt(average_sq_x);
   data[1] = app_int_sqrt(average_sq_y) ;
   data[2] = app_int_sqrt(average_sq_z);
   
   return app_ok;
   
}

#ifdef APP_VIBERATE_ABS
static APP_Status_t app_viberate_abs(uint32_t* data)
{
    int16_t i;
    int32_t average_x = 0, average_y = 0, average_z = 0;
    int32_t sum_x = 0, sum_y = 0, sum_z = 0;
    uint32_t sum_abs_x = 0, sum_abs_y = 0, sum_abs_z = 0;
    
    /*sum*/
    for(i = 0; i < AXIS_BUFFER_SIZE; i++){
        sum_x += axis_buf[i].axis_x;
        sum_y += axis_buf[i].axis_y;
        sum_z += axis_buf[i].axis_z;
        
    }
   /*average*/
   average_x = sum_x / AXIS_BUFFER_SIZE;
   average_y = sum_y / AXIS_BUFFER_SIZE;
   average_z = sum_z / AXIS_BUFFER_SIZE;
   /*sum of square*/
   for(i = 0; i < AXIS_BUFFER_SIZE; i++){
       sum_abs_x += abs((int32_t)axis_buf[i].axis_x-average_x);
       sum_abs_y += abs((int32_t)axis_buf[i].axis_y-average_y);
       sum_abs_z += abs((int32_t)axis_buf[i].axis_z-average_z);
   }
   /*average*/
   data[0] = sum_abs_x / AXIS_BUFFER_SIZE;
   data[1] = sum_abs_y / AXIS_BUFFER_SIZE;
   data[2] = sum_abs_z / AXIS_BUFFER_SIZE;
   
   return app_ok;
}
#endif

static int32_t app_int_sqrt(uint32_t data)
{
    int32_t r = 0, t = 0;
    int8_t i = 0;
    
    for(i = 15; i >= 0; i--){
        t = r | (int32_t)( 0x01 << i);
        if(((t) * (t)) < data){
            r = t;
        }
    }
    
    return r;
}


/**
 * @brief  EXTI line detection callback.
 * @param  Specifies the pins connected EXTI line
 * @retval None
 */

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
    switch (GPIO_Pin){
     case BNRG_SPI_IRQ_PIN:
         HCI_Isr();
     break;
     case SHT3X_DIS_ALERT_INT_PIN:
         
     break;
     case LIS2DH12_INT2_PIN:
         
     break;
     case LIS2DH12_INT1_PIN:
        if(LIS2DH12_X_Y_Z_Get_Axes_Raw() != 0x00)
            while(1);
     break;
        
     default:
     break;
    }
}

void app_i2c_dma_rx_cmp_callback(uint8_t* data)
{
    static int16_t axis_flag = 0, count = 0;
    
    for(count = 0; count < LIS2DH12_SAMPLE_NUMBER; count++){
        axis_buf[count+axis_flag].axis_x = ((((int16_t)data[(count * 6)+1]) << 8) + (int16_t)data[(count * 6)+0]);
        axis_buf[count+axis_flag].axis_y = ((((int16_t)data[(count * 6)+3]) << 8) + (int16_t)data[(count * 6)+2]);
        axis_buf[count+axis_flag].axis_z = ((((int16_t)data[(count * 6)+5]) << 8) + (int16_t)data[(count * 6)+4]);
    }
    axis_flag += LIS2DH12_SAMPLE_NUMBER;
    if(axis_flag == AXIS_BUFFER_SIZE){
            
#ifdef APP_TILT_ANGLE
        app_tilt_angle_output();
#endif
#ifdef APP_VIBERATE
        app_viberate_value_output();
#endif
#ifdef APP_FFT
        app_fft_output();   
#endif
        axis_flag = 0;
        app_3axis_int_callback();
    } 
}

/*flash***************************************************************************************/
void app_led_toggle_times(uint8_t times)
{
    uint8_t i;
    for(i = 0; i < times; i++)
    {
        BSP_LED_Toggle(LED5);
        HAL_Delay(500);
        BSP_LED_Toggle(LED5);
        HAL_Delay(500);
    }

}

/*OTA***************************************************************************************/

void app_get_firmware_version(uint8_t* data)
{
    uint32_t version;
   
    version = *(__IO uint32_t*)CURRENT_FIRMWARE_VERSION_INFO_ADDRESS;
   
    app_bit_shift_to_byte(data, &version, 4);
}  

void app_write_firmware_version(uint8_t* data)
{
    uint32_t version;
 
    app_bit_shift_uint8_to_uint32(data, &version);
    if(flash_page_write(NEW_FIRMWARE_VERSION_INFO_ADDRESS, &version, 1) != app_ok)
        while(1);
    
    
}

void app_ota_reset(void)
{
    /* Reset manager will take care of running the new application*/
    HAL_NVIC_SystemReset();
}

void app_file_rx(uint8_t* data, uint8_t len)
{
    app_new_firmware_manager(data, len);    
      
}

static void app_file_flash_write_over(void)
{
    uint8_t ret, tmp = 0x01;
    
    ret = aci_gatt_update_char_value(file_tx_serv_handle, file_tx_notify_char_handle, 0, 1, &tmp);
    if (ret != app_ok)
        while(1);
}

void app_crc_value(uint8_t data)
{
    uint8_t ret;
    
    ret = aci_gatt_update_char_value(crc_serv_handle, crc_read_char_handle, 0, 1, &data);
    if (ret != app_ok)
        while(1);
}

static void app_new_firmware_manager(uint8_t*data, uint8_t len)
{
    uint8_t crc_value;
    static uint8_t count = 0, info_flag = 0;
    static flash_write_info_t data_info;
    static uint8_t tmp_buf[OTA_WRITE_FLASH_BYTE_NUMBER] = {0};
    
    if(info_flag == 0){
     
        app_bit_shift_uint8_to_uint32(data, &data_info.address);
        data_info.size = data[4];
        info_flag = 1;
    }else{
        memcpy(&tmp_buf[count], data, len);
        count+= len;
        if(count == data_info.size){
            crc_value = SHT3X_CalcCrc(tmp_buf, data_info.size);
            app_crc_value(crc_value);
            if(app_write_new_firmware_to_flash(tmp_buf, data_info.size, data_info.address) != app_ok)
                while(1);
            
            count = 0;
            info_flag = 0;
            memset(tmp_buf, 0, data_info.size);
        }
    }
    
    app_led_toggle_times(1);
   
}

static APP_Status_t app_write_new_firmware_to_flash(uint8_t* data, uint8_t len, uint32_t address)
{
    static uint32_t tmp_buf[OTA_WRITE_FLASH_WORD_NUMBER];
    uint32_t word_count = 0;
    uint8_t i,tmp;
    
    tmp = len;
    if(tmp < OTA_WRITE_FLASH_BYTE_NUMBER){
        do{
            data[tmp] = 0;
            tmp++;
        }while(OTA_WRITE_FLASH_BYTE_NUMBER - tmp);
    }
    
    for(i = 0; i < OTA_WRITE_FLASH_WORD_NUMBER; i++){
        app_bit_shift_uint8_to_uint32(&data[i * 4], &tmp_buf[word_count]);
        word_count++;
    }
 
    if(flash_page_write(address, tmp_buf, OTA_WRITE_FLASH_PAGE_NUMBER) != app_ok)
        return app_error;
    
    memset(tmp_buf, 0, OTA_WRITE_FLASH_WORD_NUMBER); 
    /*notify upper write finish*/
    app_file_flash_write_over();

    return app_ok;
}

static void app_ble_write_flash_crc_test_1_record_data(uint32_t* data, uint32_t address)
{
      if(flash_page_content_check(address, data, 1) == app_ok)
          app_led_toggle_times(1);   
}



